#include "zf_common_headfile.h"

#ifdef DRV8701//如果是8701的驱动
void Speed_Init (void)
{
    //有刷PWM初始化
    gpio_init(MOTOR_L_EN, GPO, GPIO_LOW, GPO_PUSH_PULL);
    pwm_init(MOTOR_L_PH, 12000, 0); //PWM引脚初始化
    gpio_init(MOTOR_R_EN, GPO, GPIO_LOW, GPO_PUSH_PULL);
    pwm_init(MOTOR_R_PH, 12000, 0);//PWM引脚初始化
//    gtm_pwm_init(ATOM0_CH7_P20_8, 12000, 0);
    //无刷控制的PWM初始化
    pwm_init(MOTR_LEFT_WUSHA_PIN, 12000, 0);
    pwm_init(MOTR_RIGHT_WUSHA_PIN, 12000, 0);
}

/*
 * @函数功能：设置电机输出函数
 * @参数(int Left):左边电机的PWM值，范围为-999~999
 * @参数(int Right):右边电机的PWM值，范围为-999~999
 * @作者：吴广金
 * @时间：2024-3-1
 * */
void Speed_Out (int Left, int Right)
{

    if (Left >= 0)
    {
        if (Left > 999)
            Left = 999;
        pwm_set_duty(MOTOR_L_PH, Left);//设置PWM输出
        gpio_set_level(MOTOR_L_EN, 0);//设置输出方向ֹ
    }
    else
    {
        if (Left < -999)
            Left = -999;
        pwm_set_duty(MOTOR_L_PH, ~Left);//设置PWM输出
        gpio_set_level(MOTOR_L_EN, 1);//设置输出方向
    }

    if (Right >= 0)
    {
        if (Right > 999)
            Right = 999;
        pwm_set_duty(MOTOR_R_PH, Right);//设置PWM输出
        gpio_set_level(MOTOR_R_EN, 0);//设置输出方向ֹ
    }
    else
    {
        if (Right < -999)
            Right = -999;
        pwm_set_duty(MOTOR_R_PH, ~Right);//设置PWM输出
        gpio_set_level(MOTOR_R_EN, 1);//设置输出方向
    }
}

/*
 *  @电机测试函数
 *  @作者：吴广金
 *  @时间：2024-3-1
 * */
void speed_test(void)
{
    static int Key_data = 0;
    static int Pwm_L = 0,Pwm_R = 0;
    while(1)
    {
        Key_data=Key_Read();
        if(Key_data==1)
        {
            Pwm_L+=50;
        }
        if(Key_data==2)
        {
            Pwm_L-=50;
        }
        if(Key_data==3)
        {
            Pwm_R+=50;
        }
        if(Key_data==4)
        {
            Pwm_R-=50;
        }
        oled_show_int(80,4,(int32)Pwm_L,5);
        oled_show_int(80,3,(int32)Pwm_R,5);
        Speed_Out(Pwm_L,Pwm_R);
    }

}
#else//普通驱动
void Speed_Init (void)
{
    //有刷PWM初始化
    pwm_init(MOTOR_LEFT_POSITIVE_PIN, 12000, 0); //ATOM 0ģ���ͨ��7 ʹ��P21_2�������PWM  PWMƵ��12000HZ  ռ�ձȰٷ�֮1000/GTM_ATOM0_PWM_DUTY_MAX*100
    pwm_init(MOTOR_LEFT_NEGATIVE_PIN, 12000, 0);

    pwm_init(MOTOR_RIGHT_POSITIVE_PIN, 12000, 0);
    pwm_init(MOTOR_RIGHT_NEGATIVE_PIN, 12000, 0);
//    gtm_pwm_init(ATOM0_CH7_P20_8, 12000, 0);
    //无刷控制的PWM初始化
    pwm_init(MOTR_LEFT_WUSHA_PIN, 12000, 0);
    pwm_init(MOTR_RIGHT_WUSHA_PIN, 12000, 0);
}

void Speed_Out (int Left, int Right) //���PWM��ֵ���Լ�����ת����
{
    if (Left >= 0)
    {
        if (Left > 999)
            Left = 999;
        pwm_set_duty(MOTOR_LEFT_POSITIVE_PIN, Left);        // ͨ�� 1 ���
        pwm_set_duty(MOTOR_LEFT_NEGATIVE_PIN, 0);           // ͨ��2 ֹͣ
    }
    else
    {
        if (Left < -999)
            Left = -999;
        pwm_set_duty(MOTOR_LEFT_POSITIVE_PIN, 0);           // ͨ��1 ֹͣ
        pwm_set_duty(MOTOR_LEFT_NEGATIVE_PIN, ~Left);       // ͨ�� 2 ���
    }

    if (Right >= 0)
    {
        if (Right > 999)
            Right = 999;
        pwm_set_duty(MOTOR_RIGHT_POSITIVE_PIN, Right);       // ͨ��3 ���
        pwm_set_duty(MOTOR_RIGHT_NEGATIVE_PIN, 0);           // ͨ��4 ֹͣ
    }
    else
    {
        if (Right < -999)
            Right = -999;
        pwm_set_duty(MOTOR_RIGHT_POSITIVE_PIN, 0);           // ͨ�� 3 ֹͣ
        pwm_set_duty(MOTOR_RIGHT_NEGATIVE_PIN, ~Right);      // ͨ�� 4 ���
    }
}
#endif
void Speed_MachineControl(void){

  float Machine_SumOut=0;

  if(Balance_Test) {
      if(Balance_Test == 1) {             //
          Machine_SumOut = Balance_Speed_Out;
          Machine_LeftOut = Machine_RightOut=(int)((Balance_Speed_Out)*1.0);
      } else if(Balance_Test == 2) {      //ֻ��ת��
          Machine_SumOut = TurnSpeed_Out;
          Machine_LeftOut  =  (int)(Machine_SumOut * 1.0);
          Machine_RightOut = -(int)(Machine_SumOut * 1.0);
      } else if(Balance_Test == 3) {      //ֹͣ������
          Machine_LeftOut = Machine_RightOut = 0;
      }
      else if(Balance_Test == 4) {
//      Machine_LeftOut = Machine_RightOut = 0;
//                    Machine_LeftOut=(int)Pid_Control_Speed_Z(Encoder_LeftSpeed);
//                    Machine_RightOut=(int)Pid_Control_Speed_Y(Encoder_RightSpeed);
      }
      else if(Balance_Test == 5) {
          Machine_LeftOut = Machine_RightOut = 0;
          Machine_LeftOut=(int)100+(int)TurnSpeed_Out;
          Machine_RightOut=(int)100-(int)TurnSpeed_Out;
      }
  } else { //������

      Machine_SumOut   = (int)Balance_Speed_Out;
      Machine_LeftOut  = (int)Machine_SumOut + (int)ZL_jiaosudu.output + (int)TurnSpeed_Out;
      Machine_RightOut = (int)Machine_SumOut + (int)ZL_jiaosudu.output - (int)TurnSpeed_Out;
  }

  Speed_Out(Machine_LeftOut,Machine_RightOut+35);//����ٶ����

}



